#include "vesc.h"
#include "stdio.h"

VescPacket vesc_packet[255];


#define MAX_DUTY 95000 //95%



int32_t erpm;


int32_t s32_limit(int32_t value, int32_t maxVal, int32_t minVal)
{
    if(value > maxVal) {
        value = maxVal;
    }
    else if(value < minVal) {
        value = minVal;
    }

    return value;
}

void vesc_can_rx_disp(uint32_t ExtId, uint8_t RxMsg[])
{
    switch(ExtId >> 8) {
        case CAN_PACKET_STATUS: {
            vesc_packet[ExtId & 0xff].erpm = (RxMsg[0] << 24) | (RxMsg[1] << 16) | (RxMsg[2] << 8) | RxMsg[3];
            vesc_packet[ExtId & 0xff].duty = (RxMsg[6] << 24) | RxMsg[7];
        }
        break;

        case CAN_PACKET_STATUS_4: {
            vesc_packet[ExtId & 0xff].pos = (RxMsg[6] << 8) | RxMsg[7];
//            printf("%x\t%x\t%x\r\n", ExtId, RxMsg[6], RxMsg[7]);
        }
        break;
    }


}



HAL_StatusTypeDef vesc_can_send_msg(CAN_HandleTypeDef* _hcan, uint32_t ExtId, uint8_t pData[], uint8_t len)
{
    static CAN_TxHeaderTypeDef Vesc_TxHeader;

    Vesc_TxHeader.StdId = 0x00000000U;
    Vesc_TxHeader.ExtId = ExtId;
    Vesc_TxHeader.IDE = CAN_ID_EXT;
    Vesc_TxHeader.RTR = CAN_RTR_DATA;
    Vesc_TxHeader.DLC = len;
    uint32_t pTxMailbox;
    int time = 0;

    while(HAL_CAN_GetTxMailboxesFreeLevel( _hcan ) == 0) {
        time++;

        if(time > 500) {
            return HAL_ERROR;
        }
    }

    return HAL_CAN_AddTxMessage(_hcan, &Vesc_TxHeader, pData, &pTxMailbox);
}


//����ռ�ձȣ�5000->50%
HAL_StatusTypeDef Vesc_Set_Duty(CAN_HandleTypeDef* _hcan, uint8_t vesc_id, int32_t duty)
{
    duty = s32_limit(duty, MAX_DUTY, -MAX_DUTY);
    uint8_t data[4] = {duty >> 24, duty >> 16, duty >> 8, duty};
    return vesc_can_send_msg(_hcan, vesc_id | (CAN_PACKET_SET_DUTY << 8), data, 4);
}

HAL_StatusTypeDef Vesc_Set_Brake_Current(CAN_HandleTypeDef* _hcan, uint8_t vesc_id, int32_t brakeCurrent)
{
    uint8_t data[4] = {brakeCurrent >> 24, brakeCurrent >> 16, brakeCurrent >> 8, brakeCurrent};
    return vesc_can_send_msg(_hcan, vesc_id | (CAN_PACKET_SET_CURRENT_BRAKE << 8), data, 4);
}


//��λ��mA
HAL_StatusTypeDef Vesc_Set_Current(CAN_HandleTypeDef* _hcan, uint8_t vesc_id, int32_t current)
{
    uint8_t data[4] = {current >> 24, current >> 16, current >> 8, current};
    return vesc_can_send_msg(_hcan, vesc_id | (CAN_PACKET_SET_CURRENT << 8), data, 4);
}

//2022-5-26, MTO6355, AS5047p, һȦ18000
HAL_StatusTypeDef Vesc_Set_Pos(CAN_HandleTypeDef* _hcan, uint8_t vesc_id, int32_t pos)
{
    uint8_t data[4] = {pos >> 24, pos >> 16, pos >> 8, pos};
    return vesc_can_send_msg(_hcan, vesc_id | (CAN_PACKET_SET_POS << 8), data, 4);
}



//��λ��ERPM
HAL_StatusTypeDef Vesc_Set_ERPM(CAN_HandleTypeDef* _hcan, uint8_t vesc_id, int32_t erpm)
{
    uint8_t data[4] = {erpm >> 24, erpm >> 16, erpm >> 8, erpm};
    return vesc_can_send_msg(_hcan, vesc_id| (CAN_PACKET_SET_RPM << 8), data, 4);
}
//��λ��ERPM
HAL_StatusTypeDef Vesc_Set_DJI(CAN_HandleTypeDef* _hcan, uint8_t vesc_id, int32_t erpm)
{
    uint8_t data[4] = {erpm >> 24, erpm >> 16, erpm >> 8, erpm};
    return vesc_can_send_msg(_hcan, vesc_id| (CAN_PACKET_SET_RPM << 8), data, 4);
}
